#include "CollisionManager.h"


CollisionManager::CollisionManager(list<Particle> *p)
{
	particles = p;
}

CollisionManager::CollisionManager()
{
}

CollisionManager::~CollisionManager()
{
}

void CollisionManager::addWall(Vec3f p1, Vec3f p2, Vec3f p3, Vec3f p4) {
	Wall w;
	w.p1 = p1;
	w.p2 = p2;
	w.p3 = p3;
	w.p4 = p4;
	blocks.push_back(w);
}

void CollisionManager::drawWalls() {
	origin = vl_0;
	glPushMatrix();
	//glLoadIdentity();
	glTranslatef(origin[0], origin[1], origin[2]);
	for(list<Wall>::iterator w = blocks.begin(); w != blocks.end(); ++w) {
		glBegin(GL_QUADS);
		glVertex3f(w->p1[0], w->p1[1], w->p1[2]);
		glVertex3f(w->p2[0], w->p2[1], w->p2[2]);
		glVertex3f(w->p3[0], w->p3[1], w->p3[2]);
		glVertex3f(w->p4[0], w->p4[1], w->p4[2]);
		glEnd();
	}
	glPopMatrix();
}

int particleWallHit(Vec3f p1, Vec3f p2, Vec3f r1, 
				Vec3f r2, Vec3f r3, Vec3f r4) {
	Vec3f u = p2 - p1;
	Vec3f w = p1 - r1;
	float d = dot(cross(r2 - r1, r4 - r1), u);
	float n = -1 * dot(cross(r2 - r1, r4 - r1), w);
	
	if(fabs(d) < 0.001) {
		if(n == 0) {
			//	in plane
			return -1;
		} else {
			//	no intersect
			return -2;
		}
	} else {
		float s = n / d;
		if(s < 0 || s > 1) {
			//	nope
			return -3;
		} else {
			//	inside the rectangle?
			Vec3f p = p1 + s * p2;
			if(0 <= dot(p - r1, r2 - r1) && dot(p - r1, r2 - r1) <= dot(r2 - r1, r2 - r1) &&
			   0 <= dot(p - r1, r4 - r1) && dot(p - r1, r4 - r1) <= dot(r4 - r1, r4 - r1)) {
				return 1;
			} else {
				return -4;
			}
		}
	}
}

enum RobotArmControls
{ 
    BASE_ROTATION=0,DISPLACEMENT, LOWER_TILT_LEFT, LOWER_TILT_RIGHT, /*UPPER_TILT,*/ CLAW_ROTATION,
    NECK_LENGTH, UPPER_LENGTH_L, LOWER_LENGTH_L, UPPER_LENGTH_R, LOWER_LENGTH_R, HAND_ANGLE_L,
    HAND_ANGLE_R, COMPOUND_L, COMPOUND_R, WALL1, WALL2, WALL3,

    NUMCONTROLS, 
};

void CollisionManager::update(float dt) {
	
	//	check wall by wall
	Vec3f p1, p2, newV, v, n, d;
	int checks = 0;
	int ignores = 0;
	int walls = 0;
	bool spoke = false;
	Wall wall;
	float rest = ModelerApplication::Instance()->GetControlValue(WALL1);
	
	for(list<Wall>::iterator w = blocks.begin(); w != blocks.end(); ++w) {
		wall.p1 = w->p1 + origin;
		wall.p2 = w->p2 + origin;
		wall.p3 = w->p3 + origin;
		wall.p4 = w->p4 + origin;
		
		n = cross(wall.p2 - wall.p1, wall.p4 - wall.p1);
		normalise(n);
		walls++;
		if(walls == 2)
			rest = ModelerApplication::Instance()->GetControlValue(WALL2);
		else if(walls == 3) 
			rest = ModelerApplication::Instance()->GetControlValue(WALL3);
		else
			rest = ModelerApplication::Instance()->GetControlValue(WALL1);
			
		for(list<Particle>::iterator p = particles->begin(); p != particles->end(); ++p) {
			p1 = p->getPosition();
			p2 = p1 + 4*dt * p->getVelocity();
			float dist = dot(norm(n), p1 - wall.p1);
			

			if(fabs(dist) > 1) {
				spoke = true;
				//cout << "Ignored " << fabs(len(p1 - w->p2)) << endl;
				ignores++;
			} else {
				checks++;
				int ret = particleWallHit(p1, p2, wall.p1, wall.p2, wall.p3, wall.p4);
				if(ret == 1) {
					//	update particle velocity
					v = p->getVelocity();
					newV = 2 * (dot(-1 * v, n)) * n + v;
					
					p->setVelocity(newV * rest);
				} else {
					//cout << "Check miss " << ret << endl;
				}
			}	
		}
	}
	//cout << "Checked " << checks << " times" << endl;
	//cout << "Ignored " << ignores << " times" << endl;
}

